package com.gitee.wsl.mathematics.geometry.d2.curve.ext

import com.gitee.wsl.mathematics.vector.vec2.Vec2f

fun derivative(x0: Vec2f, c0: Vec2f, x1: Vec2f, t: Double): Vec2f {
    val it = 1.0 - t
    return Vec2f(2 * it * (c0.x - x0.x) + 2 * t * (x1.x - c0.x), 2 * it * (c0.y - x0.y) + 2 * t * (x1.y - c0.y))
}

/**
 * Similar to [derivative] but handles cases in which [p0] and [p1] coincide.
 */
fun safeDerivative(p0: Vec2f, c0: Vec2f, p1: Vec2f, t: Double): Vec2f {
    val epsilon = 10E-6
    var u = t

    val d10 = c0 - p0
    val d21 = c0 - p1

    if (u < epsilon && d10.squaredLength < epsilon) {
        u = epsilon
    }

    if (u > (1.0 - epsilon) && d21.squaredLength < epsilon) {
        u = 1.0 - epsilon
    }

    val iu = 1.0 - u
    return Vec2f(2 * iu * (c0.x - p0.x) + 2 * u * (p1.x - c0.x), 2 * iu * (c0.y - p0.y) + 2 * u * (p1.y - c0.y))
}


fun derivative(p0: Vec2f, p1: Vec2f, p2: Vec2f, p3: Vec2f, t: Double): Vec2f {
    val it = 1.0 - t
    return p1.minus(p0).times(3.0 * it * it).plus(p2.minus(p1).times(6.0 * it * t)).plus(p3.minus(p2).times(3.0 * t * t))
}


fun derivative2(p0: Vec2f, p1: Vec2f, p2: Vec2f, p3: Vec2f, t: Double): Vec2f {
    return (p2 - p1 * 2.0 + p0) * (6 * (1.0 - t)) + (p3 - p2 * 2.0 + p1) * (6.0 * t)
}


/**
 * Similar to [derivative] but handles cases in which [p0] and [p1] or [p2] and [p3] coincide.
 */
fun safeDerivative(p0: Vec2f, p1: Vec2f, p2: Vec2f, p3: Vec2f, t: Double): Vec2f {
    val epsilon = 10E-6
    var u = t

    val d10 = p1 - p0
    val d32 = p3 - p2

    if (u < epsilon && d10.squaredLength < epsilon) {
        u = epsilon
    }

    if (u > (1.0 - epsilon) && d32.squaredLength < epsilon) {
        u = 1.0 - epsilon
    }

    val iu = 1.0 - u
    return ((d10 * (3.0 * iu * iu)) + (p2 - p1) * (6.0 * iu * u)) + d32 * (3.0 * u * u)
}

fun normal(x0: Vec2f, c0: Vec2f, x1: Vec2f, t: Double): Vec2f {
    val (x, y) = derivative(x0, c0, x1, t)
    return Vec2f(-y, x).normalized
}

fun bezier(x0: Vec2f, c0: Vec2f, x1: Vec2f, t: Double): Vec2f {
    val it = 1.0 - t
    val it2 = it * it
    val t2 = t * t

    return Vec2f(
        it2 * x0.x + 2 * it * t * c0.x + t2 * x1.x,
        it2 * x0.y + 2 * it * t * c0.y + t2 * x1.y
    )
}

fun bezier(x0: Vec2f, c0: Vec2f, c1: Vec2f, x1: Vec2f, t: Double): Vec2f {
    val it = 1.0 - t
    val it2 = it * it
    val it3 = it2 * it
    val t2 = t * t
    val t3 = t2 * t

    return Vec2f(
        it3 * x0.x + 3 * it2 * t * c0.x + 3 * it * t2 * c1.x + t3 * x1.x,
        it3 * x0.y + 3 * it2 * t * c0.y + 3 * it * t2 * c1.y + t3 * x1.y)
}
